#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H

#include "Matrix.h"

class KalmanFilter {
public:
    KalmanFilter(double R, double Q);
    void updateStateTransitionMatrix(double dt);
    void predict();
    void extrapolate();
    void update(const Vector2& z);
    Vector4 getState() const;
    Vector4 getStateEx() const;

private:
    Vector4 x, x_ex;      // State vector [x, y, vx, vy]
    Matrix4x4 F, P, Q;    // State transition matrix, covariance matrix, process noise covariance
    Matrix2x4 H;          // Measurement matrix
    Matrix2x2 R;          // Measurement noise covariance
};

#endif // KALMAN_FILTER_H 